# -*- coding: utf-8 -*-
"""
Calibrate the Camera with Zhang Zhengyou Method.
Picture File Folder: "./pic/RGB_camera_calib_img/", Without Distortion. 

By You Zhiyuan, 2022.07.04, zhiyuanyou@foxmail.com
"""

import os
import numpy as np
import json
from calibrate_helper import Calibrator


def main():
    img_dir = "calibration\\2025-10-24\\images"
    intrinsics_dir = "calibration\\intrinsics.json"
    shape_inner_corner = (6, 4)
    size_grid = 0.038
    # create calibrator
    calibrator = Calibrator(img_dir, shape_inner_corner, size_grid)
    # calibrate the camera
    # mat_intri, coff_dis = calibrator.calibrate_camera()
    with open(intrinsics_dir, 'r') as f:
        intrinsics = json.load(f)
    K_list = []
    D_list = []
    for intrinsic in intrinsics:
        K = np.array(intrinsic['intrinsic'])
        fx, fy = K[0, 0], K[1, 1]
        cx, cy = K[0, 2], K[1, 2]
        new_cx = cy
        new_cy = 1280 - 1 - cx

        new_K = np.array([
            [fy, 0, new_cx],
            [0, fx, new_cy],
            [0, 0, 1]
        ], dtype=np.float32)
        K_list.append(new_K)
        D_list.append(np.array(intrinsic['distortion'], dtype=np.float32))

    calibrator.calibrate_camera_with_intrinsic(K_list,D_list)

if __name__ == '__main__':
    main()
